#ifndef SE2_H
#define SE2_H

#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif

#include <Eigen/Dense>
#include <iostream>

struct PreSE2{
public:
    double meas[3];
    double cov[9]; // 3*3, RowMajor
};

struct Se2
{
    float x;
    float y;
    float theta;
    Se2();
    Se2(float _x, float _y ,float _theta);
    ~Se2() ;
    Se2 inv() const;
    Se2 operator -(const Se2& that) const;
    Se2 operator +(const Se2& that) const;
    // cv::Mat toCvSE3() const;
    Se2& fromCvSE3(const Eigen::Matrix4d& mat);
};

#endif // SE2_H
